﻿#include "algorithmvision.h"
#include <vector>
#include <algorithm>
#include <opencv2/imgproc/imgproc_c.h>

void AlgorithmVision::HomMat3dFromRVT(const cv::Mat& Rvec, const cv::Mat& Tvec, cv::Mat* HomMat3d)
{
	assert(Rvec.rows == 3 && Rvec.cols == 1 && Tvec.rows == 3 && Tvec.cols == 1);
	cv::Mat RMatrix;
	Rodrigues(Rvec, RMatrix);
	HomMat3dFromRT(RMatrix, Tvec, HomMat3d);
}

void AlgorithmVision::HomMat3dToRVT(const cv::Mat& HomMat3d, cv::Mat* Rvec, cv::Mat* Tvec)
{
	assert(HomMat3d.rows == 4 && HomMat3d.cols == 4);
	cv::Mat RMatrix;
	HomMat3dToRT(HomMat3d, &RMatrix, Tvec);
	Rodrigues(RMatrix, *Rvec);
}
//opencv构建齐次矩阵进行位姿变换
//cv::Mat RMatrix;
//Rodrigues(rvec3, RMatrix);
//cv::Mat Tvec = tvec3;
//cv::Mat HomMat3D;
//HomMat3dFromRT(RMatrix, Tvec, HomMat3D);
void AlgorithmVision::HomMat3dFromRT(const cv::Mat& RMatrix, const cv::Mat& Tvec, cv::Mat* HomMat3d)
{
	assert(RMatrix.rows == 3 && RMatrix.cols == 3 && Tvec.rows == 3 && Tvec.cols == 1);
	//assert(RMatrix.depth == CV_64FC1 && Tvec.depth == CV_64FC1);//需要保持类型相同
	*HomMat3d = cv::Mat::eye(4, 4, CV_64FC1);
	cv::Mat SubMat = (*HomMat3d)(cv::Range(0, 3), cv::Range(0, 3));//获取区间索引
	RMatrix.copyTo(SubMat);//需要保持类型相同
	cv::Mat SubMat2 = (*HomMat3d)(cv::Range(0, 3), cv::Range(3, 4));
	Tvec.copyTo(SubMat2);
}

void AlgorithmVision::HomMat3dToRT(const cv::Mat& HomMat3d, cv::Mat* RMatrix, cv::Mat* Tvec)
{
	assert(HomMat3d.rows == 4 && HomMat3d.cols == 4);
	//assert(RMatrix.depth == CV_64FC1 && Tvec.depth == CV_64FC1);//需要保持类型相同
	cv::Mat SubMat = HomMat3d(cv::Range(0, 3), cv::Range(0, 3));//获取区间索引
	SubMat.copyTo(*RMatrix);
	cv::Mat SubMat2 = HomMat3d(cv::Range(0, 3), cv::Range(3, 4));
	SubMat2.copyTo(*Tvec);
}
void AlgorithmVision::HomMat3dCompose(const cv::Mat& HomMat3DLeft, const cv::Mat& HomMat3DRight, cv::Mat* HomMat3DCompose)
{
	assert(HomMat3DLeft.rows == 4 && HomMat3DLeft.cols == 4);
	assert(HomMat3DRight.rows == 4 && HomMat3DRight.cols == 4);
	(*HomMat3DCompose) = HomMat3DLeft * HomMat3DRight;
}


//绘制一些线
void AlgorithmVision::lines(cv::InputOutputArray img, const std::vector<cv::Point>& vpt1, const std::vector<cv::Point>& vpt2, const cv::Scalar& color, int thickness, int lineType, int shift)
{
	//检测

	for (int i = 0; i < vpt1.size(); i++) {
		line(img, vpt1[i], vpt2[i], color, thickness, lineType, shift);
	}
}
void AlgorithmVision::lines(cv::InputOutputArray img, const std::vector<cv::Point2f>& vpt1, const std::vector<cv::Point2f>& vpt2, const cv::Scalar& color, int thickness, int lineType, int shift)
{
	//检测

	for (int i = 0; i < vpt1.size(); i++) {
		line(img, vpt1[i], vpt2[i], color, thickness, lineType, shift);
	}
}

void AlgorithmVision::Disp3dCoordSystem(cv::Mat* DrawImage, const cv::Mat& HomMat3dPose, const cv::Mat& CameraMatrix, const cv::Mat& DistCoeffs, const float& fAxialLength, const int& iAxiaThickness, const std::string& sCoordName, const bool& bShowCoordPoseText)
{
	//检测

	//转换 绘制
	cv::Mat Rvec, Tvec;
	HomMat3dToRVT(HomMat3dPose, &Rvec, &Tvec);
	Disp3dCoordSystem(DrawImage, Rvec, Tvec, CameraMatrix, DistCoeffs, fAxialLength, iAxiaThickness, sCoordName, bShowCoordPoseText);
}
//Mat cameraMatrix;//相机内参
//Mat distCoeffs;//畸变系数
//绘制函数
//绘制空间坐标系
void AlgorithmVision::Disp3dCoordSystem(cv::Mat* DrawImage, const cv::Mat& Rvec, const cv::Mat& Tvec, const cv::Mat& CameraMatrix, const cv::Mat& DistCoeffs, const float& fAxialLength, const int& iAxiaThickness, const std::string& sCoordName, const bool& bShowCoordPoseText)
{
	//检测


	//绘制空间坐标系（利用投影）
	//涉及位姿计算
	std::vector<cv::Point3f> DrawObjectPoints;//空间箭头绘制点
	//--//float fAxialLength = 100;//坐标轴长度mm
	DrawObjectPoints.push_back(cv::Point3f(0, 0, 0));
	DrawObjectPoints.push_back(cv::Point3f(fAxialLength, 0, 0));//X
	DrawObjectPoints.push_back(cv::Point3f(0, fAxialLength, 0));//Y
	DrawObjectPoints.push_back(cv::Point3f(0, 0, fAxialLength));//Z
	std::vector<cv::Point2f> DrawImagePoints;//绘制箭头投影图像点
	cv::projectPoints(DrawObjectPoints, Rvec, Tvec, CameraMatrix, DistCoeffs, DrawImagePoints);
	cv::Scalar color = cv::Scalar(0, 0, 255);
	//--//int iAxiaThickness = 3;
	line(*DrawImage, DrawImagePoints[0], DrawImagePoints[1], color, iAxiaThickness, cv::LINE_AA);
	color = cv::Scalar(0, 255, 0);
	line(*DrawImage, DrawImagePoints[0], DrawImagePoints[2], color, iAxiaThickness, cv::LINE_AA);
	color = cv::Scalar(255, 0, 0);
	line(*DrawImage, DrawImagePoints[0], DrawImagePoints[3], color, iAxiaThickness, cv::LINE_AA);

	if (sCoordName != "") {
		cv::Point2f temp = DrawImagePoints[0];
		temp.y = temp.y - 10;
		temp.x = temp.x;
		//绘制坐标系文字
		putText(*DrawImage, sCoordName, temp, CV_FONT_HERSHEY_COMPLEX, 1, cv::Scalar(255, 0, 0), 3, 8);
	}
	if (bShowCoordPoseText == true) {
		//显示坐标系的坐标值
		float x, y, z;

		if (Tvec.type() == CV_32F) {
			x = Tvec.at<float>(0, 0);
			y = Tvec.at<float>(1, 0);
			z = Tvec.at<float>(2, 0);
		} else {
			x = Tvec.at<double>(0, 0);
			y = Tvec.at<double>(1, 0);
			z = Tvec.at<double>(2, 0);
		}
		std::string sCoordPoseText = "   ";
		sCoordPoseText = sCoordPoseText + "(" + std::to_string(x) + "," + std::to_string(y) + "," + std::to_string(z) + ")";
		putText(*DrawImage, sCoordPoseText, DrawImagePoints[0], CV_FONT_HERSHEY_COMPLEX_SMALL, 0.6, cv::Scalar(12, 23, 200), 1, 8);
	}
}
